//
// Created by sukai on 22-9-19.
//

#include <iostream>
#include <ros/ros.h>
#include <signal.h>

#include <queue>
#include <vector>
#include <unordered_map>


using namespace std;
/**
 *
 * @param argc
 * @param argv
 * @return
起始点 id = 0；
目标点 id = 5；
现在需要 从 execute_map_plan_中找出所有的路径 ，起始点 id 到 目标点 id 的所有路径，
每条路径都按查找顺序依次放在一个集合中，然后输出路径长度最短的路径。
起始点 id = 0，目标点 id = 5，execute_map_plan_中找出所有能到达的路径 ，那么输出的路径 例如：0->1->5，0->2->3->4->5，这些路径都是起始点 id = 0，目标点 id = 5的路径，但是路径长度最短的路径为：0->5，所以输出的路径为：0->5。
 * 2023-01-10
 * sukai
 * rosrun contnav_global_planner test3_contnav_contnav_global_planner_node
 */
// 用广度优先搜索（BFS）算法来找出起始点到目标点的所有路径，并计算每条路径的长度，然后选择路径长度最短的路径作为结果输出。
class MorPlanToMap {//封装多条路径【储存像素】，包括起点，终点，路径点，路径长度
public:
    int plan_id; // 当前路径id
    std::vector<int> next_plan_ids; // 当前路径行走结束后，可以到达的路径id
    MorPlanToMap() {}
    MorPlanToMap(int plan_id, std::vector<int> &next_plan_ids) {
        this->plan_id = plan_id; // 路径唯一id
        this->next_plan_ids = next_plan_ids; // 当前路径行走结束后，可以到达的路径id
    }
};


//ctrl + c
void mySigintHandler(int sig)
{
    std::cout << "ctrl c  sig: "<<sig<<std::endl;

    ros::shutdown();
    if (ros::isShuttingDown()){
        cout<<"close ros  test.cpp"<<endl;
    }
}
//================================


// 使用广度优先搜索（BFS）算法来找出起始点到目标点的所有路径，并计算每条路径的长度，然后选择路径长度最短的路径作为结果输出。
void find_shortest_path(int start_id, int end_id, std::map<int, MorPlanToMap> &map_plan,std::vector<std::vector<int>> &all_paths) {
    std::queue<std::vector<int>> q; // 定义BFS使用的队列，每个元素是一个存储路径的vector
    q.push({start_id}); // 将起始点存储为第一个元素
    // std::vector<std::vector<int>> all_paths; // 存储所有路径; [sukai 提取出来] 存储所有路径的id
    int shortest_path_length = INT_MAX; // 存储路径长度最短的路径长度
    while (!q.empty()) {
        std::vector<int> path = q.front(); // 取出队列中的第一个元素（路径）
        q.pop();
        int last_node = path.back(); // 取出路径中的最后一个节点
        if (last_node == end_id) { // 如果最后一个节点是目标节点，则将此路径存储到all_paths中 【 已经找到一条从start到end的路径】
            all_paths.push_back(path);
            if (path.size() < shortest_path_length) { // 如果此路径比之前存储的路径更短，则更新最短路径长度
                shortest_path_length = path.size();
            }
        } else {
            MorPlanToMap current_plan = map_plan[last_node]; // 获取当前节点能到达的路径
            for (int next_node_id : current_plan.next_plan_ids) { // 遍历当前节点能到达的路径
                if (std::find(path.begin(), path.end(), next_node_id) == path.end()) { // 如果此路径没有被访问过，则将此路径加入到队列中
                    std::vector<int> new_path = path;
                    new_path.push_back(next_node_id);
                    q.push(new_path);
                }
            }
        }
    }

    // 输出所有路径
    for (std::vector<int> path : all_paths) {
        std::cout << "Path: ";
        for (int node_id : path) {
            std::cout << node_id << "->";
        }
        std::cout << "End\n";
    }

    // 输出最短路径
    std::cout << "输出最短路径 Shortest path: ";
    for (int node_id : all_paths[0]) {
        std::cout << node_id << "->";
    }
    std::cout << "End\n";
}








// rosrun contnav test2_node
int main(int argc,char *argv[]){
    //节点名
    string nodeName = "test2_contnav_contnav_global_planner_node";
    //初始化节点
    ros::init(argc,argv,nodeName);
    //创建节点
    ros::NodeHandle node;
    //监控 ctrl c
    signal(SIGINT, mySigintHandler);

//=================================

    vector<int>  v1 = {1,23,4,5,6,67};
    vector<int>  v2 = {3,4,5,6,7,7};
    vector<int>  v3;
    v3.insert(v3.end(), v1.begin(), v1.end());
    v3.insert(v3.end(), v2.begin(), v2.end());


    for (int node_id : v3) {
        std::cout <<"v3: "<< node_id << "->";
    }
    std::cout << "End\n";


    std::map<int, MorPlanToMap> execute_map_plan_;
    std::vector<int> next_plan_ids;
    next_plan_ids.push_back(1);
    next_plan_ids.push_back(2);
    next_plan_ids.push_back(3);
    MorPlanToMap morPlanToMap(0, next_plan_ids);
    execute_map_plan_.insert(std::pair<int, MorPlanToMap>(0, morPlanToMap));

    next_plan_ids.clear();
    next_plan_ids.push_back(0);
    next_plan_ids.push_back(4);
    next_plan_ids.push_back(5);
    MorPlanToMap morPlanToMap1(1, next_plan_ids);
    execute_map_plan_.insert(std::pair<int, MorPlanToMap>(1, morPlanToMap1));

    next_plan_ids.clear();
    next_plan_ids.push_back(3);
    MorPlanToMap morPlanToMap2(2, next_plan_ids);
    execute_map_plan_.insert(std::pair<int, MorPlanToMap>(2, morPlanToMap2));

    next_plan_ids.clear();
    next_plan_ids.push_back(4);
    next_plan_ids.push_back(5);
    MorPlanToMap morPlanToMap3(3, next_plan_ids);
    execute_map_plan_.insert(std::pair<int, MorPlanToMap>(3, morPlanToMap3));

    next_plan_ids.clear();
    next_plan_ids.push_back(0);
    next_plan_ids.push_back(3);
    MorPlanToMap morPlanToMap4(4, next_plan_ids);
    execute_map_plan_.insert(std::pair<int, MorPlanToMap>(4, morPlanToMap4));

    next_plan_ids.clear();
    next_plan_ids.push_back(0);
    next_plan_ids.push_back(1);
    next_plan_ids.push_back(4);
    MorPlanToMap morPlanToMap5(5, next_plan_ids);

    int start_id = 0;
    int target_id = 5;

    std::vector<std::vector<int>> all_paths; // 存储所有路径; [sukai 提取出来] 存储所有路径的id
    //std::unordered_set<int> visited;
    find_shortest_path(start_id, target_id, execute_map_plan_,all_paths);


//    int main(int argc, char **argv) {
//        ros::init(argc, argv, "test3_node",ros::init_options::NoSigintHandler);
//        ros::NodeHandle nh;
//        signal(SIGINT, mySigintHandler);
//        //================================
//        std::map<int, MorPlanToMap> map_plan;
//        map_plan[0] = MorPlanToMap(0, {1, 2});
//        map_plan[1] = MorPlanToMap(1, {3, 4});
//        map_plan[2] = MorPlanToMap(2, {3, 4});
//        map_plan[3] = MorPlanToMap(3, {5});
//        map_plan[4] = MorPlanToMap(4, {5});
//        map_plan[5] = MorPlanToMap(5, {});
//        find_shortest_path(0, 5, map_plan);
//        //================================
//        ros::spin();
//        return 0;
//    }






//========================================
//    cout << "C++打印出当前master所处的hostname: " <<host << endl;
//    cout << "C++打印出当前master运行在的端口: " <<port << endl;
//    cout << "C++打印出当前master的URL地址: " <<uri << endl;
//    cout << "C++打印出当前检查master是否启动: " <<checkfig<< endl;

//    C++打印出当前master所处的hostname: localhost
//    C++打印出当前master运行在的端口: 11311
//    C++打印出当前master的URL地址: http://localhost:11311
//    C++打印出当前检查master是否启动: 1


    //ros入门教程
    //https://blog.csdn.net/Chen_2018k/article/details/104334367
    //事件轮询
    ros::spin();
    return 0;
}

/**
  class MorPlanToMap {//封装多条路径【储存像素】，包括起点，终点，路径点，路径长度
    public:

        int plan_id;;//当前路径id
        std::vector<int> next_plan_ids;//当前路径行走结束后，可以到达的路径id

       MorPlanToMap() {}

        MorPlanToMap(int &plan_id, std::vector<int> &next_plan_ids) {
            this->plan_id =plan_id;//路径唯一id
            this->next_plan_ids = next_plan_ids;//当前路径行走结束后，可以到达的路径id
        }

    };

  map<int, MorPlanToMap> execute_map_plan_;//需要执行的多个路径
    void init_execute_map_plan() {//初始化数据
        std::vector<int> next_plan_ids;
        next_plan_ids.push_back(1);
        next_plan_ids.push_back(2);
        next_plan_ids.push_back(3);
        MorPlanToMap morPlanToMap(0, next_plan_ids);//第0条路径可以到达1,2,3,的路径上
        execute_map_plan_.insert(std::pair<int, MorPlanToMap>(0, morPlanToMap));

        next_plan_ids.clear();
        next_plan_ids.push_back(0);
        next_plan_ids.push_back(4);
        next_plan_ids.push_back(5);
        MorPlanToMap morPlanToMap1(1, next_plan_ids);//第1条路径可以到达0,2,3,4,5,6的路径上
        execute_map_plan_.insert(std::pair<int, MorPlanToMap>(1, morPlanToMap1));

        next_plan_ids.clear();
        next_plan_ids.push_back(3);
        MorPlanToMap morPlanToMap2(2, next_plan_ids);//第2条路径可以到达1,0,7,8,9的路径上
        execute_map_plan_.insert(std::pair<int, MorPlanToMap>(2, morPlanToMap2));

        next_plan_ids.clear();
        next_plan_ids.push_back(4);
        next_plan_ids.push_back(5);
        MorPlanToMap morPlanToMap3(3, next_plan_ids);
        execute_map_plan_.insert(std::pair<int, MorPlanToMap>(3, morPlanToMap3));

        next_plan_ids.clear();
        next_plan_ids.push_back(0);
        next_plan_ids.push_back(3);
        MorPlanToMap morPlanToMap4(4, next_plan_ids);
        execute_map_plan_.insert(std::pair<int, MorPlanToMap>(4, morPlanToMap4));

        next_plan_ids.clear();
        next_plan_ids.push_back(0);
        next_plan_ids.push_back(1);
        next_plan_ids.push_back(4);
        MorPlanToMap morPlanToMap5(5, next_plan_ids);
        execute_map_plan_.insert(std::pair<int, MorPlanToMap>(5, morPlanToMap5));
        }



 */
